#include "can_device_interface/controlcan.h"
#include <ros/ros.h>
#include <std_msgs/String.h>
#include <std_msgs/Int64.h>
#include "can_device_interface/canmsg.h"


void sub_callback(const can_device_interface::canmsg::ConstPtr &msg);
// 初始化设备信息
int nDeviceType = 4; /* USBCAN-2A或USBCAN-2C或CANalyst-II */
int nDeviceInd = 0;  /* 第1个设备 */
int nCANInd = 0;     /* 第1个通道 */
int n_channel = 2;
#define uint8 unsigned short int

int main(int argc, char **argv)
{
    // 初始化ROS节点
    ros::init(argc, argv, "can_device_interface");
    ros::NodeHandle n;
    ros::Publisher can_raw_pub;
    ros::Subscriber can_raw_sub;
    can_raw_pub = n.advertise<can_device_interface::canmsg>("can_raw", 100);
    can_raw_sub = n.subscribe("control_msg", 100, sub_callback);
    ros::Rate loop_rate(10);
    //************************************************************
    // 启动设备
    DWORD state;
    state = VCI_OpenDevice(nDeviceType, nDeviceInd, 0);
    if (state != 1)
    {
        ROS_INFO("The Can device fails to be opened!");
        return -1;
    }
    else
    {
        ROS_INFO("The Can device is successfully opened!");
    }
    //************************************************************
    // 依次初始化设备通道并启动
    VCI_INIT_CONFIG config;
    config.AccMask = 0xFFFFFFFF;
    config.Filter = 1;     // 接收所有帧
    config.Timing0 = 0x00; // 波特率500 Kbps  0x00  0x1C
    config.Timing1 = 0x1C;
    config.Mode = 0; // 正常模式
    for (int channel_id = 0; channel_id <n_channel; channel_id++)
    {
        // 初始化通道
        state = VCI_InitCAN(nDeviceType, nDeviceInd, channel_id, &config);
        if (state != 1)
        {
            ROS_INFO("The Can device channel %d fails to be initialized!", channel_id);
            VCI_CloseDevice(nDeviceType, nDeviceInd);
            return -1;
        }
        else
        {
            ROS_INFO("The Can device channel %d is successfully initialized!", channel_id);
        }
        // 启动
        state = VCI_StartCAN(nDeviceType, nDeviceInd, channel_id);
        if (state != 1)
        {
            ROS_INFO("The Can device channel %d fails to be started!", channel_id);
            VCI_CloseDevice(nDeviceType, nDeviceInd);
            return -1;
        }
        else
        {
            ROS_INFO("The Can device channel %d is successfully started!", channel_id);
        }

    }
       
    int seq = 0;
    int reclen = 0;
    while (ros::ok)
    {
        // 初始化msg信息
        can_device_interface::canmsg msg;
        // 从设备读取原始报文
        VCI_CAN_OBJ rec[3000]; // 接收缓存，设为3000为佳。
        //从通道1接收数据
        for (int channel_id = 0; channel_id <n_channel; channel_id++)
        {
            reclen = VCI_Receive(nDeviceType, nDeviceInd, channel_id, rec, 3000, 100); // 返回值为实际读取的帧数
            if (reclen > 0)
            {
                for (int i = 0; i < reclen; i++)
                {
                    msg.header.frame_id = "can_raw";
                    msg.header.seq = seq;
                    msg.header.stamp = ros::Time::now();
                    // 设备信息
                    msg.device_type = "USBCAN-2A,USBCAN-2C or CANalyst-II";
                    msg.device_id = 0;
                    msg.channel_id = channel_id;
                    msg.device_type_id = 4;

                    for (int j = 0; j < 8; j++)
                    {
                        msg.data[j] = rec[i].Data[j];
                    }
                    msg.flag = rec[i].RemoteFlag;
                    msg.len = rec[i].DataLen;
                    msg.id = rec[i].ID;
                    can_raw_pub.publish(msg);
                    seq++;
                    if (seq >= 10000)
                        seq = 0;
                }
            }
        }
        ros::spinOnce();
        loop_rate.sleep();
    }
    state=VCI_CloseDevice(nDeviceType, nDeviceInd);
    if (state != 1)
    {
        ROS_INFO("The Can device fails to be closed!");
        return -1;
    }
    else
    {
        ROS_INFO("The Can device is successfully closed!");
    }

    return 0;
}

void sub_callback(const can_device_interface::canmsg::ConstPtr &msg)
{
    ROS_INFO("frame ID = %d", msg->header.frame_id);
    // 将msg写入设备
    // state = VCI_Transmit(nDeviceType, nDeviceInd, nCANInd, vco,48);
    VCI_CAN_OBJ transmit_msg;
    transmit_msg.ID=msg->id;
    transmit_msg.SendType=0;
	transmit_msg.RemoteFlag=0;
	transmit_msg.ExternFlag=1;
	transmit_msg.DataLen=msg->len;
    for (int i=0;i<msg->len;i++)
    {
        transmit_msg.Data[i]=msg->data[i];
    }
    
    DWORD state=0;
    state = VCI_Transmit(msg->device_type_id, msg->device_id, msg->channel_id, &transmit_msg, 1);
    if (state != 1)
    {
        ROS_INFO("CAN message send successed,channel id=%d",msg->channel_id);
    }
    else
    {
        ROS_INFO("CAN message send failed");
    }
}